#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "fdecl.h"

range_datapoint *read_ascii_xyz_data(const char *filename, int *num_data_points,
				     double strength_data_force)
{
  FILE *f;
  int i;
  double x, y, z;
  range_datapoint *buffer;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  fscanf(f, "%d", num_data_points);
  buffer = (range_datapoint *) calloc(*num_data_points, 
				      sizeof(range_datapoint));
    
  for (i = 0; i < *num_data_points; i++) {
    fscanf(f, "%lf %lf %lf",
	   &buffer[i].position.x, 
	   &buffer[i].position.y, 
	   &buffer[i].position.z);
    buffer[i].strength_force = strength_data_force;
  }
  return buffer;
}

int read_ascii_xyz_data_hybroids(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j;
  range_datapoint *buffer;
  hybroid *h;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",
	   &buffer[j].position.x, 
	   &buffer[j].position.y, 
	   &buffer[j].position.z);
      buffer[j].strength_force = strength_data_force;
    }
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

int read_ascii_xyz_data_hybroids_missrow(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j, count =0;
  range_datapoint *buffer;
  hybroid *h;
  double x,y,z;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",&x, &y, &z);

      if((j!=(9*11+1))&& (j!=(9*11+2))&&  (j!=(9*11+3))&&  (j!=9*11+4)&& 
         (j!=9*11+5)&&  (j!=9*11+6)
          ) {
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
      }
    }
    h->numdatapoints = count;
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

int read_ascii_xyz_data_hybroids_sparse(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j, count =0;
  range_datapoint *buffer;
  hybroid *h;
  double x,y,z;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",&x, &y, &z);

      if((j%3)==0){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
      }
      if(j==(numdatapoints-1)){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }
      if(j==(numdatapoints-2)){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }
    }
    h->numdatapoints = count;
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

int read_ascii_xyz_data_hybroids_sparse_20(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j, count =0;
  range_datapoint *buffer;
  hybroid *h;
  double x,y,z;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    count = 0;
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",&x, &y, &z);
/* the j!=0 was used in the 5-bend data */
      if((j%4)==0 && j!=4 && j!=24 && j!=0){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
      }
      if(j==(numdatapoints-1)){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }

      if(j==(numdatapoints-2)){
	   buffer[count].position.x = x; 
	   buffer[count].position.y = y; 
	   buffer[count].position.z = z;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }

    }
    h->numdatapoints = count;
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

int read_ascii_xyz_data_hybroids_sparse_20_noise(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j, count =0;
  range_datapoint *buffer;
  hybroid *h;
  double x,y,z;
  double noise = 0.1;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",&x, &y, &z);

      if((j%4)==0 && j!=4 && j!=24){
	if((j%2)==0){
		   buffer[count].position.x = x + x * noise; 
	   buffer[count].position.y = y - y * noise; 
	   buffer[count].position.z = z + z * noise;
   	if(fabs(z) < 0.1){
   	buffer[count].position.z = z + 0.1;
   	buffer[count].position.y = y - 0.1;
   	buffer[count].position.x = x + 0.1;
      }
     	 }
	else{
	   buffer[count].position.x = x - x * noise; 
	   buffer[count].position.y = y + y * noise; 
	   buffer[count].position.z = z - z * noise;
   	if(fabs(z) < 0.1){
   	buffer[count].position.z = z - 0.4;
   	buffer[count].position.y = y - 0.4;
      }
	 }
      buffer[count].strength_force = strength_data_force;
      count++;
      }
      if(j==(numdatapoints-1)){
	   buffer[count].position.x = x+x * noise; 
	   buffer[count].position.y = y+y * noise; 
	   buffer[count].position.z = z+ z * 0.3 * noise;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }

      if(j==(numdatapoints-2)){
	   buffer[count].position.x = x-x * noise; 
	   buffer[count].position.y = y-y * noise; 
	   buffer[count].position.z = z- z * noise;
      buffer[count].strength_force = strength_data_force;
      count++;
	 }

    }
    h->numdatapoints = count;
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

int read_ascii_xyz_data_hybroids_noise(const char *filename, hybroid *hybroids[], 
                                  int numhybroids, double strength_data_force)
{

  FILE *f;
  int i, j, l;
  double x, y, z, p;
  double noise = 0.15;
  range_datapoint *buffer;
  hybroid *h;
  int numdatapoints;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  
  for(i = 0; i < numhybroids; i++) {
    h = hybroids[i];
    fscanf(f, "%d", &h->numdatapoints);
    numdatapoints = h->numdatapoints;
    buffer = (range_datapoint *) calloc(numdatapoints, 
				      sizeof(range_datapoint));
    for(j = 0; j < numdatapoints; j++){
      fscanf(f, "%lf %lf %lf",
	   &x, &y, &z);
      p = rand();
      if(p > 0.5) l = 1;
      if(p <= 0.5) l = 0;
      if(l == 0){
      if((l=j % 2) == 0){
	buffer[j].position.x = x + x * noise;
	buffer[j].position.y = y + y * noise;
	buffer[j].position.z = z + z * noise;
	if(fabs(buffer[j].position.z - 0.0) < 0.01)
	buffer[j].position.z = z + 0.1;
	if(fabs(buffer[j].position.z + 2.7) < 0.1)
	buffer[j].position.z = z - 0.1;
      }
      else{
	buffer[j].position.x = x - x * noise;
	buffer[j].position.y = y - y * noise;
	buffer[j].position.z = z - z * noise;
	if(fabs(buffer[j].position.z - 0.0) < 0.01)
	buffer[j].position.z = z - 0.1;
      }
      }
      else {
      if((l=j % 2) == 1){
	buffer[j].position.x = x + x * noise;
	buffer[j].position.y = y + y * noise;
	buffer[j].position.z = z + z * noise;
	if(fabs(buffer[j].position.z - 0.0) < 0.01)
	buffer[j].position.z = z + 0.1;
	if(fabs(buffer[j].position.z + 2.7) < 0.1)
	buffer[j].position.z = z - 0.1;
      }
      else{
	buffer[j].position.x = x - x * noise;
	buffer[j].position.y = y - y * noise;
	buffer[j].position.z = z - z * noise;
	if(fabs(buffer[j].position.z - 0.0) < 0.01)
	buffer[j].position.z = z - 0.1;
      }
    }
      buffer[j].strength_force = strength_data_force;
    }
    h->range_data = buffer;
    if (h->range_data == NULL) h->numdatapoints = 0;
    printf("Number of range datapnts of Hybroid[%d]= %d\n", i, h->numdatapoints);
  }

}

void initialize_datapoints_uv_to_sphere
  (hybroid *h)
{
  int i;
  double u, v, distance, initial_radius = h->sc * h->a1;
  vector p, *t = &h->translation;
  range_datapoint *d, *data = h->range_data;

  for (i = 0; i < h->numdatapoints; i++) {
    d = &data[i];
    vminus(d->position, *t, p);
    compute_nearest_point_on_sphere(&p, &u, &v, 
				    initial_radius, &distance);
    d->u = u; d->v = v;
  }
}
    

/*
vector *read_ascii_xyz_data(char *filename, int *num_data_points)
{
  FILE *f;
  int i;
  double x, y, z;
  vector *buffer;

  if ((f = fopen(filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", filename);
    return NULL;
  }

  fscanf(f, "%d", num_data_points);
  buffer = (vector *) calloc(*num_data_points, sizeof(vector));
    
  for (i = 0; i < *num_data_points; i++) 
    fscanf(f, "%lf %lf %lf", &buffer[i].x, &buffer[i].y, &buffer[i].z);

  return buffer;
}
*/

void scale_range_data(range_datapoint *data, int num_data_points,
		      double xscale, double yscale, double zscale)
{
  int i;
  vector *p;

  for (i = 0; i < num_data_points; i++) {
    p = &data[i].position;
    p->x *= xscale; p->y *= yscale; p->z *= zscale;
  }
}


void compute_range_data_center_of_gravity
  (hybroid *h, vector *cog)
{
  int i;
  vector *p;
  range_datapoint *data = h->range_data;

  vzeroize(*cog);
  for (i = 0; i < h->numdatapoints; i++) {
    p = &data[i].position;
    vinc(*p, *cog);
  }
  vscale(1.0 / h->numdatapoints, *cog, *cog);
}


unsigned char *read_obvius_image(const char *filename, int *xsize, int *ysize)
{  
  FILE *f;
  char descriptor_filename[100], data_filename[100];
  unsigned char *buffer;
    
  sprintf(descriptor_filename, "%s/desc", filename);

  if ((f = fopen(descriptor_filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", descriptor_filename);
    return NULL;
  }

  fscanf(f, "%d %d", ysize, xsize);
  fclose(f);

  sprintf(data_filename, "%s/data", filename);

  if ((f = fopen(data_filename, "r")) == NULL) {
    printf("Input file %s can't be opened\n", data_filename);
    return NULL;
  }

  buffer = (unsigned char *) calloc(*xsize * *ysize, sizeof(unsigned char));
  fread((char *)buffer, sizeof(unsigned char), *xsize * *ysize, f);
  return buffer;
}

